function theta = ikine(T, q0, DH_para, wrist)

Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;
Sigma = DH_para.Sigma;
a2 = A(2); a3 = A(3); a4 = A(4);
d1 = D(1); d3 = D(3); d4 = D(4); d6 = D(6);
theta0 = q0 + Theta;

T0e = T;
nx = T0e(1, 1); ox = T0e(1, 2); ax = T0e(1, 3); px = T0e(1, 4);
ny = T0e(2, 1); oy = T0e(2, 2); ay = T0e(2, 3); py = T0e(2, 4);
nz = T0e(3, 1); oz = T0e(3, 2); az = T0e(3, 3); pz = T0e(3, 4);

Px = px - d6 * ax;
Py = py - d6 * ay;
Pz = pz - d6 * az - d1;

theta1_1 = atan2(d3, sqrt(Px^2 + Py^2 - d3^2)) + atan2(Py, Px);
theta1_2 = atan2(d3, -sqrt(Px^2 + Py^2 - d3^2)) + atan2(Py, Px);

if abs(theta1_1 - theta0(1)) <= abs(theta1_2 - theta0(1))
    theta1 = theta1_1;
else
    theta1 = theta1_2;
end

k3 = ((a2 - cos(theta1) * Px - sin(theta1) * Py)^2 + Pz^2 - (a4^2 + d4^2 + a3^2)) / (2 * a3);
theta3_1 = atan2(k3, sqrt(a4^2 + d4^2 - k3^2)) - atan2(a4, d4);
theta3_2 = atan2(k3, -sqrt(a4^2 + d4^2 - k3^2)) - atan2(a4, d4);
if abs(theta3_1 - theta0(3)) <= abs(theta3_2 - theta0(3))
    theta3 = theta3_1;
else
    theta3 = theta3_2;
end

g = cos(theta1) * Px + sin(theta1) * Py - a2;
e = a4 * cos(theta3) + d4 * sin(theta3) + a3;
f = a4 * sin(theta3) - d4 * cos(theta3);
if g^2 + Pz^2 > 0
    theta2 = atan2(Pz * e - g * f, g * e + Pz * f);
else
    theta2 = pi / 2;
end

T01 = transformation_mdh(Alpha(1), A(1), D(1), 0, Sigma(1), theta1);
T12 = transformation_mdh(Alpha(2), A(2), D(2), 0, Sigma(2), theta2);
T23 = transformation_mdh(Alpha(3), A(3), D(3), 0, Sigma(3), theta3);
T03 = T01 * T12 * T23;

T36 = pinv(T03) * T0e;
theta5_1 = acos( - T36(2, 3));
if isreal(theta5_1)
    theta5_2 = - theta5_1;
    if wrist == 0
        theta5 = theta5_1;
    else
        theta5 = theta5_2;
    end
else
    theta5 = 0;
end

if sin(theta5) ~= 0
    theta4 = atan2(T36(3, 3) / sin(theta5), T36(1, 3) / sin(theta5));
else
    theta4 = theta0(4);
end

if sin(theta5) ~= 0
    theta6 = atan2(-T36(2, 2) / sin(theta5), T36(2, 1) / sin(theta5));
else
    theta6 = atan2(T36(3, 1), T36(1, 1)) - theta4;
end

theta = [theta1, theta2, theta3, theta4, theta5, theta6] - Theta;
